#include<iostream>
#include<opencv2/imgcodecs.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/objdetect.hpp>
#include<opencv2/opencv.hpp>
#include<string>
#include<vector>
#include"func.h"

using namespace std;
using namespace cv;

cv::KalmanFilter initializeKalmanFilter()
{
    // 状态向量包括x和y坐标，速度的x和y分量
    cv::KalmanFilter kalmanFilter(4, 2, 0);

    // 状态转移矩阵，描述状态间的转移关系
    kalmanFilter.transitionMatrix = (cv::Mat_<float>(4, 4) << 1, 0, 1, 0,
                                                               0, 1, 0, 1,
                                                               0, 0, 1, 0,
                                                               0, 0, 0, 1);

    // 测量矩阵，将状态映射到观测值
    kalmanFilter.measurementMatrix = (cv::Mat_<float>(2, 4) << 1, 0, 0, 0,
                                                               0, 1, 0, 0);

    // 控制矩阵，可能对系统进行外部控制，这里未使用
    kalmanFilter.controlMatrix = cv::Mat::zeros(4, 2, CV_32F);

    // 测量噪声协方差矩阵
    kalmanFilter.measurementNoiseCov = (cv::Mat_<float>(2, 2) << 0.1, 0,
                                                                  0, 0.1);

    // 过程噪声协方差矩阵
    kalmanFilter.processNoiseCov = (cv::Mat_<float>(4, 4) << 0.1, 0,    0,    0,
                                                              0,    0.1, 0,    0,
                                                              0,    0,    0.01, 0,
                                                              0,    0,    0,    0.01);

    // 初始化状态向量和协方差矩阵
    kalmanFilter.statePost = cv::Mat::zeros(4, 1, CV_32F);
    kalmanFilter.errorCovPost = (cv::Mat_<float>(4, 4) << 1,   0,   0,   0,
                                                            0,   1,   0,   0,
                                                            0,   0,   1,   0,
                                                            0,   0,   0,   1);

    return kalmanFilter;
}

cv::Point2f predictNextPosition(cv::KalmanFilter& kalmanFilter)
{
    // 预测下一状态
    cv::Mat prediction = kalmanFilter.predict();

    // 提取预测的x和y坐标
    cv::Point2f predictedPosition(prediction.at<float>(0), prediction.at<float>(1));

    return predictedPosition;
}

cv::Point2f updateKalmanFilter(cv::KalmanFilter& kalmanFilter, const cv::Point2f& measurement)
{
    // 更新卡尔曼滤波器的状态
    cv::Mat measurementMat = (cv::Mat_<float>(2, 1) << measurement.x, measurement.y);
    cv::Mat estimated = kalmanFilter.correct(measurementMat);

    // 提取更新后的x和y坐标
    cv::Point2f estimatedPosition(estimated.at<float>(0), estimated.at<float>(1));

    return estimatedPosition;
}

Point2f yuce(Point2f objectCenter,bool firstf)
{

        // 初始化Kalman滤波器
        cv::KalmanFilter kalmanFilter = initializeKalmanFilter();
        cv::Point2f correctedPosition;

        
        if(firstf)
        {
            kalmanFilter.statePre.at<float>(0) = objectCenter.x;  // x坐标
            kalmanFilter.statePre.at<float>(1) = objectCenter.y;  // y坐标
        }
        

        // 预测下一个状态
        cv::Point2f predictedPosition = predictNextPosition(kalmanFilter);

        // 更新Kalman滤波器状态
        correctedPosition = updateKalmanFilter(kalmanFilter, objectCenter);
        
    
    return correctedPosition;

}
//